#include <Lucnx/interrupt.h>
#include <Lucnx/printk.h>
#include <Lucnx/task.h>
#include <Lucnx/debug.h>
#include <Lucnx/mutex.h>
#include <Lucnx/types.h>

void semp_init(semaphore_t *semp)
{
    list_init(&semp->waiters);
    semp->value = 1;
}

void semp_down(semaphore_t *semp)
{
    bool old_state = set_interrupt_state(false);
    task_t *current = running_task();
    while (!semp->value)
    {
        list_add_end(&semp->waiters, &current->task_node);
        task_block(running_task(), TASK_BLOCKED); 
    }
    assert(semp->value == 1)
    semp->value--;
    assert(!semp->value);
    set_interrupt_state(old_state);
}

void semp_up(semaphore_t *semp)
{
    bool old_state = set_interrupt_state(false);
    task_t *cur = running_task();
    assert(!semp->value);
    semp->value++;
    assert(semp->value == 1);
    list_node *waiter_node = list_pop(&semp->waiters);
    if (waiter_node == NULL)
    {
        set_interrupt_state(old_state);
        return;
    }
    task_t *waiter = member2entry(task_t, task_node, waiter_node);
    task_unblock(waiter);
    set_interrupt_state(old_state);
}


void lock_acquire(lock_t *lock)
{
    task_t *cur = running_task();
    if (lock->holder != cur)
    {
        semp_down(&lock->semp);
        assert(!lock->acquire_nr);
        assert(!lock->semp.value);
        lock->holder = cur;
        lock->acquire_nr = 1;
    }
    else
    {
        lock->acquire_nr++;
    }
}

void lock_release(lock_t *lock)
{
    assert(lock->holder == running_task());
    if (lock->acquire_nr == 1)
    {
        lock->acquire_nr--;
        lock->holder = NULL;
        semp_up(&lock->semp);
    }
    else
    {
        lock->acquire_nr--;
    }
}

void lock_init(lock_t *lock)
{
    bool old_state = set_interrupt_state(false);
    lock->acquire_nr = 0;
    lock->holder = NULL;
    semp_init(&lock->semp);
    set_interrupt_state(old_state);
}